//
// Created by mouse on 2019/12/19.
//

#include "app_stepper.h"

AccelStepper stepper1(AccelStepper::FULL4WIRE, D5, D7, D6, D8);
MultiStepper steppers;

static String setStepperTopic;
static String postStepperTopic;

static Ticker cmdSendTimer;

static SerialServoCmd *jsonstr_to_serial_servo_cmd(String jsonStr)
{
    auto *cmd = new SerialServoCmd();

    const size_t capacity = 2048;
    DynamicJsonDocument doc(capacity);

    deserializeJson(doc, jsonStr);

    JsonObject obj = doc.as<JsonObject>();

    String type = obj["type"];
    if (type.equals("setPosition"))
    {
        cmd->cmd = CMD_SET_POSITION;
    } else if (type.equals("setSpeed"))
    {
        cmd->cmd = CMD_SET_SPEED;
    } else if (type.equals("startActionGroup"))
    {
        cmd->cmd = CMD_START_ACTION_GROUP;
    } else if (type.equals("stopOrRestore"))
    {
        cmd->cmd = CMD_STOP_OR_RESTORE;
    }

    cmd->ch = obj["ch"];

    int32_t value = obj["value"];
    cmd->data_l = value & 0xff;
    cmd->data_h = value >> 8 & 0xff;

    return cmd;
}


void cmdSendTimerHandler(SerialServoCmd *cmd)
{
    uint8_t cmd_data[5] = {cmd->head, cmd->cmd, cmd->ch, cmd->data_l, cmd->data_h};
    Serial1.write(cmd_data, 5);

    delete cmd;
}

void StepperTask::setup()
{
    stepper1.setMaxSpeed(100);
    steppers.addStepper(stepper1);

    if (!setStepperTopic.isEmpty())
    {
        _networkTask->addSubscribeTopic(setStepperTopic, [](String &topic, String &payload) {
            SerialServoCmd *cmd = jsonstr_to_serial_servo_cmd(payload);
            cmdSendTimer.once(0, cmdSendTimerHandler, cmd);
        }, 0);
    }

}

StepperTask::StepperTask(NetworkTask *networkTask)
{
    _networkTask = networkTask;

    if (!String(configInfo->device_key).isEmpty())
    {
        auto *topic = new MqttUtils::TopicEntity();

        strcpy(topic->title, MQTT_TOPIC_USER);
        strcpy(topic->product, "*");
        strcpy(topic->device, configInfo->device_key);
        strcpy(topic->function, "set");
        strcpy(topic->item, "stepper");
        setStepperTopic = MqttUtils::toTopicString(topic);

//        strcpy(topic->title, MQTT_TOPIC_USER);
//        strcpy(topic->product, "*");
//        strcpy(topic->device, configInfo->device_key);
//        strcpy(topic->function, "post");
//        strcpy(topic->item, "stepper");
//        postStepperTopic = MqttUtils::toTopicString(topic);

        delete topic;
    }

}

void StepperTask::loop()
{
}